function [s, v, a] = Position_S_traj_7_1(stop_time, dt, set_a)
% 生成位置S曲线
% omgea, theta为期望的角速度的和位置
% stop_time为停止时间,dt为仿真步长
% set_a为设定加速度

step = stop_time/dt + 1;
a = zeros(step,1);
v = zeros(step,1);
s = zeros(step,1);
s(1) = 0.1;

for i = 2:1:step
    t = (i-1)*dt;
    if t <= 1
        a(i) = set_a;% 匀加速段
    elseif t > 1 && t <= 2
        a(i) = 0; % 匀速段
    elseif t > 2 && t <= 3
        a(i) = -set_a; % 匀减速段
    elseif t > 3
        a(i) = 0;
        v(i-1) = 0; % 静止段
    end
    v(i) = v(i-1) + a(i)*dt; % 迭代速度
    s(i) = s(i-1) + v(i)*dt; % 迭代加速度
end
end
